#include <Adafruit_GFX.h>
#include <Adafruit_GrayOLED.h>
#include <Adafruit_SPITFT.h>
#include <Adafruit_SPITFT_Macros.h>
#include <gfxfont.h>
#include <SoftwareSerial.h>
// Wire Master Writer
#include <Wire.h>   //引用i2c库文件
#include "M_Function.h"
#include "M_Declaration.h"
#include "string.h"
#include <stdio.h>
SoftwareSerial blueTooth(10, 11);
#define pulsevalue1 1430
#define pulsevalue2 1430
#define pulsevalue3 1430


#define clockwiseValue1 1217
#define clockwiseValue2 1230
#define clockwiseValue3 1217


#define anticlockwiseValue1 1617
#define anticlockwiseValue2 1630
#define anticlockwiseValue3 1617

int num = 1; //发送到编号设备
//1417是中立值，大于逆时针，小于顺时针
int setvalue1 = 1417;
int setvalue2 = 1430;
int setvalue3 = 1417;
int Claw_w_num=1;
int Claw_s_num=1;
int Claw_d_num=1;
int Mode_Flag = 0;
char RockerX = '0';
char RockerY = '0';
char RockerZ = '0';
char BT_num;
char Claw_w;
char Claw_s;
char Claw_d;
char flag_d;
char c;
void setup() {

  Serial.begin(115200);
  Wire.begin(); // 连接i2c总线（主地址可选）
  blueTooth.begin(9600);//设置软串口通讯波特率

}
void BT_Read()
{

  c = blueTooth.read();

  if ( c > 0)
  {
    Serial.print(c);
    if (c == 'x')
    {
      Mode_Flag = 1;
    }
    if (c == 'y')
    {
      Mode_Flag = 2;
    }
    if (c == 'z')
    {
      Mode_Flag = 3;
    }
    if (c == 'n')
    {
      Mode_Flag = 4;
    }
    if (c == 'w')
    {
      Mode_Flag = 5;
    }
    if (c == 's')
    {
      Mode_Flag = 6;
    }
        if (c == 'd')
    {
      Mode_Flag = 7;
    }

    if (Mode_Flag == 1 && c != 'x')
    {
      RockerX = c;
    }
    if (Mode_Flag == 2 && c != 'y')
    {
      RockerY = c;
    }
    if (Mode_Flag == 3 && c != 'z')
    {
      RockerZ = c;
    }
    if (Mode_Flag == 4 && c != 'n')
    {
      BT_num = c;
    }
    if (Mode_Flag == 5 && c != 'w')
    {
    Claw_w=c;
    }
    if (Mode_Flag == 6 && c != 's')
    {
    Claw_s=c;
    }
        if (Mode_Flag == 6 && c != 'd')
    {
    flag_d=c;
    }

  }
}
void loop() {

  BT_Read();
  num = BT_num - '0';
  JointTransmission(num, RockerX, RockerY, RockerZ,Claw_w,Claw_s,Claw_d);//启动IIC总线传输
  delay(10);
}
